#include <stdlib.h>
#include <string.h>
#include "vl53l5cx_api.h"
#include "vl53l5cx_buffers.h"

#include <stdio.h>

#include "Debugger.hpp"

/**
 * @brief Inner function, not available outside this file. This function is used
 * to wait for an answer from VL53L5CX sensor.
 */

static uint8_t _vl53l5cx_poll_for_answer(
        VL53L5CX_Configuration	*p_dev,
        uint8_t					size,
        uint8_t					pos,
        uint16_t				address,
        uint8_t					mask,
        uint8_t					expected_value)
{
    uint8_t status = VL53L5CX_STATUS_OK;
    uint8_t timeout = 0;

    do {
        status |= RdMulti(&(p_dev->platform), address,
                p_dev->temp_buffer, size);
        status |= WaitMs(&(p_dev->platform), 10);

        if(timeout >= (uint8_t)200)	/* 2s timeout */
        {
            status |= p_dev->temp_buffer[2];
            Debugger::reportForever("TIMEOUT\n");
            break; 
        }else if((size >= (uint8_t)4) 
                && (p_dev->temp_buffer[2] >= (uint8_t)0x7f))
        {
            status |= VL53L5CX_MCU_ERROR;
            Debugger::reportForever("MCU ERROR\n"); 
            break; 
        }
        else
        {
            timeout++;
        }
    } while ((p_dev->temp_buffer[pos] & mask) != expected_value);

    return status;
}

/**
 * @brief Inner function, not available outside this file. This function is used
 * to set the offset data gathered from NVM.
 */

static uint8_t _vl53l5cx_send_offset_data(
        VL53L5CX_Configuration		*p_dev,
        uint8_t						resolution)
{
    uint8_t status = VL53L5CX_STATUS_OK;
    uint32_t signal_grid[64];
    int16_t range_grid[64];
    uint8_t dss_4x4[] = {0x0F, 0x04, 0x04, 0x00, 0x08, 0x10, 0x10, 0x07};
    uint8_t footer[] = {0x00, 0x00, 0x00, 0x0F, 0x03, 0x01, 0x01, 0xE4};
    int8_t i, j;

    (void)memcpy(p_dev->temp_buffer,
            p_dev->offset_data, VL53L5CX_OFFSET_BUFFER_SIZE);

    /* Data extrapolation is required for 4X4 offset */
    if(resolution == (uint8_t)VL53L5CX_RESOLUTION_4X4){
        (void)memcpy(&(p_dev->temp_buffer[0x10]), dss_4x4, sizeof(dss_4x4));
        SwapBuffer(p_dev->temp_buffer, VL53L5CX_OFFSET_BUFFER_SIZE);
        (void)memcpy(signal_grid,&(p_dev->temp_buffer[0x3C]),
                sizeof(signal_grid));
        (void)memcpy(range_grid,&(p_dev->temp_buffer[0x140]),
                sizeof(range_grid));

        for (j = 0; j < (int8_t)4; j++)
        {
            for (i = 0; i < (int8_t)4 ; i++)
            {
                signal_grid[i+(4*j)] =
                    (signal_grid[(2*i)+(16*j)+ (int8_t)0]
                     + signal_grid[(2*i)+(16*j)+(int8_t)1]
                     + signal_grid[(2*i)+(16*j)+(int8_t)8]
                     + signal_grid[(2*i)+(16*j)+(int8_t)9])
                    /(uint32_t)4;
                range_grid[i+(4*j)] =
                    (range_grid[(2*i)+(16*j)]
                     + range_grid[(2*i)+(16*j)+1]
                     + range_grid[(2*i)+(16*j)+8]
                     + range_grid[(2*i)+(16*j)+9])
                    /(int16_t)4;
            }
        }
        (void)memset(&range_grid[0x10], 0, (uint16_t)96);
        (void)memset(&signal_grid[0x10], 0, (uint16_t)192);
        (void)memcpy(&(p_dev->temp_buffer[0x3C]),
                signal_grid, sizeof(signal_grid));
        (void)memcpy(&(p_dev->temp_buffer[0x140]),
                range_grid, sizeof(range_grid));
        SwapBuffer(p_dev->temp_buffer, VL53L5CX_OFFSET_BUFFER_SIZE);
    }

    (void)memcpy(p_dev->temp_buffer, &(p_dev->temp_buffer[8]),
            VL53L5CX_OFFSET_BUFFER_SIZE - (uint16_t)4);
    (void)memcpy(&(p_dev->temp_buffer[0x1E0]), footer, 8);
    status |= WrMulti(&(p_dev->platform), 0x2e18, p_dev->temp_buffer,
            VL53L5CX_OFFSET_BUFFER_SIZE);
    status |=_vl53l5cx_poll_for_answer(p_dev, 4, 1,
            VL53L5CX_UI_CMD_STATUS, 0xff, 0x03);

    return status;

} // _vl53l5cx_send_offset_data(

/**
 * @brief Inner function, not available outside this file. This function is used
 * to set the Xtalk data from generic configuration, or user's calibration.
 */

static uint8_t _vl53l5cx_send_xtalk_data(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				resolution)
{
    uint8_t status = VL53L5CX_STATUS_OK;
    uint8_t res4x4[] = {0x0F, 0x04, 0x04, 0x17, 0x08, 0x10, 0x10, 0x07};
    uint8_t dss_4x4[] = {0x00, 0x78, 0x00, 0x08, 0x00, 0x00, 0x00, 0x08};
    uint8_t profile_4x4[] = {0xA0, 0xFC, 0x01, 0x00};
    uint32_t signal_grid[64];
    int8_t i, j;

    (void)memcpy(p_dev->temp_buffer, &(p_dev->xtalk_data[0]),
            VL53L5CX_XTALK_BUFFER_SIZE);

    /* Data extrapolation is required for 4X4 Xtalk */
    if(resolution == (uint8_t)VL53L5CX_RESOLUTION_4X4)
    {
        (void)memcpy(&(p_dev->temp_buffer[0x8]),
                res4x4, sizeof(res4x4));
        (void)memcpy(&(p_dev->temp_buffer[0x020]),
                dss_4x4, sizeof(dss_4x4));

        SwapBuffer(p_dev->temp_buffer, VL53L5CX_XTALK_BUFFER_SIZE);
        (void)memcpy(signal_grid, &(p_dev->temp_buffer[0x34]),
                sizeof(signal_grid));

        for (j = 0; j < (int8_t)4; j++)
        {
            for (i = 0; i < (int8_t)4 ; i++)
            {
                signal_grid[i+(4*j)] =
                    (signal_grid[(2*i)+(16*j)+0]
                     + signal_grid[(2*i)+(16*j)+1]
                     + signal_grid[(2*i)+(16*j)+8]
                     + signal_grid[(2*i)+(16*j)+9])/(uint32_t)4;
            }
        }
        (void)memset(&signal_grid[0x10], 0, (uint32_t)192);
        (void)memcpy(&(p_dev->temp_buffer[0x34]),
                signal_grid, sizeof(signal_grid));
        SwapBuffer(p_dev->temp_buffer, VL53L5CX_XTALK_BUFFER_SIZE);
        (void)memcpy(&(p_dev->temp_buffer[0x134]),
                profile_4x4, sizeof(profile_4x4));
        (void)memset(&(p_dev->temp_buffer[0x078]),0 ,
                (uint32_t)4*sizeof(uint8_t));
    }

    status |= WrMulti(&(p_dev->platform), 0x2cf8,
            p_dev->temp_buffer, VL53L5CX_XTALK_BUFFER_SIZE);
    status |=_vl53l5cx_poll_for_answer(p_dev, 4, 1,
            VL53L5CX_UI_CMD_STATUS, 0xff, 0x03);

    return status;

} // _vl53l5cx_send_xtalk_data(

#ifdef VL53L5CX_LTF_FILTER

#define TARGET_STATUS_PHASECONSISTENCY 4

#define TARGET_STATUS_RANGEPHASECHECK 2
#define TARGET_STATUS_SIGMATHRESHOLDCHECK 3
#define TARGET_STATUS_MINSIGNALEVENTCHECK 8

#define TARGET_STATUS_TAILTARGETCHECK 13

#define IS_TARGET_INVALID(target_status) \
    ((target_status == TARGET_STATUS_RANGEPHASECHECK) || \
     (target_status == TARGET_STATUS_SIGMATHRESHOLDCHECK) || \
     (target_status == TARGET_STATUS_PHASECONSISTENCY) || \
     (target_status == TARGET_STATUS_MINSIGNALEVENTCHECK))

/**
 * @brief This function removed the false targets of the tail
 * @param (VL53L5CX_Configuration) *p_dev : VL53L5CX configuration structure.
 * @param (VL53L5CX_ResultsData_t) *p_results : Ranging data
 */
static uint8_t _vl53l5cx_remove_false_target(
        VL53L5CX_Configuration *p_dev,
        VL53L5CX_ResultsData *p_results) {

    uint8_t target_order, zone_id, nb_of_targets, index, false_target, nb_of_zones;
    uint32_t signal_first_target, signal;
    int16_t distance_first_target, distance;

    nb_of_zones = p_dev->resolution;
    target_order = p_dev->target_order;

    // Debugger::printf("nb_of_zones:%d, target_order:%d\n", nb_of_zones, target_order);

    if (((nb_of_zones != VL53L5CX_RESOLUTION_8X8) && (nb_of_zones != VL53L5CX_RESOLUTION_4X4)) ||
            ((target_order != VL53L5CX_TARGET_ORDER_STRONGEST) && (target_order != VL53L5CX_TARGET_ORDER_CLOSEST)))
        return VL53L5CX_STATUS_ERROR;

    // Debugger::printf("LTF called\n");

    zone_id=0;
    while (zone_id<nb_of_zones) {
        // if less than 2 tarets in this zone, nothing to be filtered out
        nb_of_targets = p_results->nb_target_detected[zone_id];
        if (nb_of_targets < 2)
            goto next_zone;

        // skip this zone if first target in the list is unvalid
        if (IS_TARGET_INVALID(p_results->target_status[zone_id*VL53L5CX_NB_TARGET_PER_ZONE]))
            goto next_zone;

        index = 1;
        if (target_order == VL53L5CX_TARGET_ORDER_STRONGEST) {
            // find closest target
            distance_first_target = p_results->distance_mm[zone_id*VL53L5CX_NB_TARGET_PER_ZONE];
            while (index < nb_of_targets) {
                // if closest is not strongest, continue to next zone
                if (p_results->distance_mm[zone_id*VL53L5CX_NB_TARGET_PER_ZONE + index] < distance_first_target)
                    // nothing to filter on this zone, go to next one
                    goto next_zone;
                index++;
            }
        }
        else {
            // find stronget and check if its also the stongest
            signal_first_target = p_results->signal_per_spad[zone_id*VL53L5CX_NB_TARGET_PER_ZONE];
            while (index < nb_of_targets) {
                // if closest is not strongest, continue to next zone
                if (p_results->signal_per_spad[zone_id*VL53L5CX_NB_TARGET_PER_ZONE + index] > signal_first_target)
                    // nothing to filter on this zone, go to next one
                    goto next_zone;
                index++;
            }
        }

        index = 1;
        signal_first_target = p_results->signal_per_spad[zone_id*VL53L5CX_NB_TARGET_PER_ZONE];
        distance_first_target = p_results->distance_mm[zone_id*VL53L5CX_NB_TARGET_PER_ZONE];
        // look if any of the following targets is a false target of the tail
        while (index < nb_of_targets) {

            if (IS_TARGET_INVALID(p_results->target_status[zone_id*VL53L5CX_NB_TARGET_PER_ZONE + index]))
                // skip this target
                goto next_index;

            signal = p_results->signal_per_spad[zone_id*VL53L5CX_NB_TARGET_PER_ZONE + index];
            distance = p_results->distance_mm[zone_id*VL53L5CX_NB_TARGET_PER_ZONE + index];

            // Debugger::printf("LT_filter : %d,%d  ", distance - distance_first_target, (signal_first_target/(signal+1)));

            false_target = 0;
            if (((distance - distance_first_target) <= 107) || (distance - distance_first_target >= 1162)) {
                if ((signal_first_target/(signal+1)) > (4*(distance - distance_first_target - 80)/100 - 2)) {
                    // Debugger::printf(" --> LT_filter case1\n");
                    false_target = 1;
                }
                else {
                    // Debugger::printf(" --> not filtered case1\n");
                }
            }
            else if ((distance - distance_first_target) <= 529) {
                if ((signal_first_target/(signal+1)) > (15*(distance - distance_first_target - 80)/100  - 5)) {
                    // Debugger::printf(" --> LT_filter case2\n");
                    false_target = 1;
                }
                else {
                    // Debugger::printf(" --> not filtered case2\n");
                }
            }
            else {
                if (((signal_first_target/(signal+1)) > (80 - (distance - distance_first_target)/30))) {
                    // Debugger::printf(" --> LT_filter case3\n");
                    false_target = 1;
                }
                else {
                    // Debugger::printf(" --> not filtered case3\n");
                }
            }

            if (false_target)
                // this is a false target of the tail
                p_results->target_status[zone_id*VL53L5CX_NB_TARGET_PER_ZONE+index] = TARGET_STATUS_TAILTARGETCHECK;

next_index:
            index++;
        }

next_zone:
        zone_id++;
    }

    return VL53L5CX_STATUS_OK;
}
#endif

uint8_t vl53l5cx_is_alive(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				*p_is_alive)
{
    uint8_t status = VL53L5CX_STATUS_OK;
    uint8_t device_id, revision_id;

    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= RdByte(&(p_dev->platform), 0, &device_id);
    status |= RdByte(&(p_dev->platform), 1, &revision_id);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);


    if ((device_id == (uint8_t)0xF0) && (revision_id == (uint8_t)0x02)) {
        *p_is_alive = 1;
    }
    else {
        *p_is_alive = 0;
    }

    return status;
}

uint8_t vl53l5cx_init(
        VL53L5CX_Configuration		*p_dev)
{
    uint8_t tmp, status = VL53L5CX_STATUS_OK;
    uint8_t pipe_ctrl[] = {VL53L5CX_NB_TARGET_PER_ZONE, 0x00, 0x01, 0x00};
    uint32_t single_range = 0x01;

    p_dev->default_xtalk = (uint8_t*)VL53L5CX_DEFAULT_XTALK;
    p_dev->default_configuration = (uint8_t*)VL53L5CX_DEFAULT_CONFIGURATION;

    /* SW reboot sequence */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0009, 0x04);
    status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
    status |= WrByte(&(p_dev->platform), 0x000A, 0x03);
    status |= RdByte(&(p_dev->platform), 0x7FFF, &tmp);
    status |= WrByte(&(p_dev->platform), 0x000C, 0x01);

    status |= WrByte(&(p_dev->platform), 0x0101, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0102, 0x00);
    status |= WrByte(&(p_dev->platform), 0x010A, 0x01);
    status |= WrByte(&(p_dev->platform), 0x4002, 0x01);
    status |= WrByte(&(p_dev->platform), 0x4002, 0x00);
    status |= WrByte(&(p_dev->platform), 0x010A, 0x03);
    status |= WrByte(&(p_dev->platform), 0x0103, 0x01);
    status |= WrByte(&(p_dev->platform), 0x000C, 0x00);
    status |= WrByte(&(p_dev->platform), 0x000F, 0x43);
    status |= WaitMs(&(p_dev->platform), 1);

    status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
    status |= WrByte(&(p_dev->platform), 0x000A, 0x01);
    status |= WaitMs(&(p_dev->platform), 100);

    /* Wait for sensor booted (several ms required to get sensor ready ) */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= _vl53l5cx_poll_for_answer(p_dev, 1, 0, 0x06, 0xff, 1);
    status |= WrByte(&(p_dev->platform), 0x000E, 0x01);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

    /* Enable FW access */
    status |= WrByte(&(p_dev->platform), 0x03, 0x0D);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
    status |= _vl53l5cx_poll_for_answer(p_dev, 1, 0, 0x21, 0x10, 0x10);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);

    /* Enable host access to GO1 */
    status |= WrByte(&(p_dev->platform), 0x0C, 0x01);

    /* Power ON status */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x101, 0x00);
    status |= WrByte(&(p_dev->platform), 0x102, 0x00);
    status |= WrByte(&(p_dev->platform), 0x010A, 0x01);
    status |= WrByte(&(p_dev->platform), 0x4002, 0x01);
    status |= WrByte(&(p_dev->platform), 0x4002, 0x00);
    status |= WrByte(&(p_dev->platform), 0x010A, 0x03);
    status |= WrByte(&(p_dev->platform), 0x103, 0x01);
    status |= WrByte(&(p_dev->platform), 0x400F, 0x00);
    status |= WrByte(&(p_dev->platform), 0x21A, 0x43);
    status |= WrByte(&(p_dev->platform), 0x21A, 0x03);
    status |= WrByte(&(p_dev->platform), 0x21A, 0x01);
    status |= WrByte(&(p_dev->platform), 0x21A, 0x00);
    status |= WrByte(&(p_dev->platform), 0x219, 0x00);
    status |= WrByte(&(p_dev->platform), 0x21B, 0x00);

    /* Wake up MCU */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0C, 0x00);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
    status |= WrByte(&(p_dev->platform), 0x20, 0x07);
    status |= WrByte(&(p_dev->platform), 0x20, 0x06);

    /* Download FW into VL53L5 */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x09);
    status |= WrMulti(&(p_dev->platform),0,
            (uint8_t*)&VL53L5CX_FIRMWARE[0],0x8000);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x0a);
    status |= WrMulti(&(p_dev->platform),0,
            (uint8_t*)&VL53L5CX_FIRMWARE[0x8000],0x8000);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x0b);
    status |= WrMulti(&(p_dev->platform),0,
            (uint8_t*)&VL53L5CX_FIRMWARE[0x10000],0x5000);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);

    /* Check if FW correctly downloaded */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);
    status |= WrByte(&(p_dev->platform), 0x03, 0x0D);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
    status |= _vl53l5cx_poll_for_answer(p_dev, 1, 0, 0x21, 0x10, 0x10);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0C, 0x01);

    /* Reset MCU and wait boot */
    status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
    status |= WrByte(&(p_dev->platform), 0x114, 0x00);
    status |= WrByte(&(p_dev->platform), 0x115, 0x00);
    status |= WrByte(&(p_dev->platform), 0x116, 0x42);
    status |= WrByte(&(p_dev->platform), 0x117, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0B, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0C, 0x00);
    status |= WrByte(&(p_dev->platform), 0x0B, 0x01);
    status |= _vl53l5cx_poll_for_answer(p_dev, 1, 0, 0x06, 0xff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

    /* Get offset NVM data and store them into the offset buffer */
    status |= WrMulti(&(p_dev->platform), 0x2fd8,
            (uint8_t*)VL53L5CX_GET_NVM_CMD, sizeof(VL53L5CX_GET_NVM_CMD));
    status |= _vl53l5cx_poll_for_answer(p_dev, 4, 0,
            VL53L5CX_UI_CMD_STATUS, 0xff, 2);
    status |= RdMulti(&(p_dev->platform), VL53L5CX_UI_CMD_START,
            p_dev->temp_buffer, VL53L5CX_NVM_DATA_SIZE);
    (void)memcpy(p_dev->offset_data, p_dev->temp_buffer,
            VL53L5CX_OFFSET_BUFFER_SIZE);
    status |= _vl53l5cx_send_offset_data(p_dev, VL53L5CX_RESOLUTION_4X4);

    /* Set default Xtalk shape. Send Xtalk to sensor */
    (void)memcpy(p_dev->xtalk_data, (uint8_t*)VL53L5CX_DEFAULT_XTALK,
            VL53L5CX_XTALK_BUFFER_SIZE);
    status |= _vl53l5cx_send_xtalk_data(p_dev, VL53L5CX_RESOLUTION_4X4);

    /* Send default configuration to VL53L5CX firmware */
    status |= WrMulti(&(p_dev->platform), 0x2c34,
            p_dev->default_configuration,
            sizeof(VL53L5CX_DEFAULT_CONFIGURATION));
    status |= _vl53l5cx_poll_for_answer(p_dev, 4, 1,
            VL53L5CX_UI_CMD_STATUS, 0xff, 0x03);
    status |= vl53l5cx_dci_write_data(p_dev, (uint8_t*)&pipe_ctrl,
            VL53L5CX_DCI_PIPE_CONTROL, (uint16_t)sizeof(pipe_ctrl));
#if VL53L5CX_NB_TARGET_PER_ZONE != 1
    tmp = VL53L5CX_NB_TARGET_PER_ZONE;
    status |= vl53l5cx_dci_replace_data(p_dev, p_dev->temp_buffer,
            VL53L5CX_DCI_FW_NB_TARGET, 16,
            (uint8_t*)&tmp, 1, 0x0C);
#endif

    status |= vl53l5cx_dci_write_data(p_dev, (uint8_t*)&single_range,
            VL53L5CX_DCI_SINGLE_RANGE,
            (uint16_t)sizeof(single_range));

#ifdef VL53L5CX_LFT_FILTER
    status |= vl53l5cx_get_target_order(p_dev, &p_dev->target_order);
    status |= vl53l5cx_get_resolution(p_dev, &p_dev->resolution);
#endif

    return status;

} // vl53l5cx_init


uint8_t vl53l5cx_set_i2c_address(
        VL53L5CX_Configuration		*p_dev,
        uint16_t		        i2c_address)
{
    uint8_t status = VL53L5CX_STATUS_OK;

    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x4, (uint8_t)(i2c_address >> 1));
    p_dev->platform.address = i2c_address;
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

    return status;
}

uint8_t vl53l5cx_get_power_mode(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				*p_power_mode)
{
    uint8_t tmp, status = VL53L5CX_STATUS_OK;

    status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
    status |= RdByte(&(p_dev->platform), 0x009, &tmp);

    switch(tmp)
    {
        case 0x4:
            *p_power_mode = VL53L5CX_POWER_MODE_WAKEUP;
            break;
        case 0x2:
            *p_power_mode = VL53L5CX_POWER_MODE_SLEEP;

            break;
        default:
            *p_power_mode = 0;
            status = VL53L5CX_STATUS_ERROR;
            break;
    }

    status |= WrByte(&(p_dev->platform), 0x7FFF, 0x02);

    return status;
}

uint8_t vl53l5cx_set_power_mode(
        VL53L5CX_Configuration		*p_dev,
        uint8_t			        power_mode)
{
    uint8_t current_power_mode, status = VL53L5CX_STATUS_OK;

    status |= vl53l5cx_get_power_mode(p_dev, &current_power_mode);
    if(power_mode != current_power_mode)
    {
        switch(power_mode)
        {
            case VL53L5CX_POWER_MODE_WAKEUP:
                status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
                status |= WrByte(&(p_dev->platform), 0x09, 0x04);
                status |= _vl53l5cx_poll_for_answer(
                        p_dev, 1, 0, 0x06, 0x01, 1);
                break;

            case VL53L5CX_POWER_MODE_SLEEP:
                status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
                status |= WrByte(&(p_dev->platform), 0x09, 0x02);
                status |= _vl53l5cx_poll_for_answer(
                        p_dev, 1, 0, 0x06, 0x01, 0);
                break;

            default:
                status = VL53L5CX_STATUS_ERROR;
                break;
        }
        status |= WrByte(&(p_dev->platform), 0x7FFF, 0x02);
    }

    return status;
}

uint8_t vl53l5cx_start_ranging(
        VL53L5CX_Configuration		*p_dev)
{
    uint8_t resolution, status = VL53L5CX_STATUS_OK;
    uint32_t i;
    uint32_t header_config[2] = {0, 0};

    union Block_header *bh_ptr;
    uint8_t cmd[] = {0x00, 0x03, 0x00, 0x00};

    status |= vl53l5cx_get_resolution(p_dev, &resolution);
    p_dev->data_read_size = 0;
    p_dev->streamcount = 255;

    /* Enable mandatory output (meta and common data) */
    uint32_t output_bh_enable[] = {
        0x00000007U,
        0x00000000U,
        0x00000000U,
        0xC0000000U};

    /* Send addresses of possible output */
    uint32_t output[] ={VL53L5CX_START_BH,
        VL53L5CX_METADATA_BH,
        VL53L5CX_COMMONDATA_BH,
        VL53L5CX_AMBIENT_RATE_BH,
        VL53L5CX_SPAD_COUNT_BH,
        VL53L5CX_NB_TARGET_DETECTED_BH,
        VL53L5CX_SIGNAL_RATE_BH,
        VL53L5CX_RANGE_SIGMA_MM_BH,
        VL53L5CX_DISTANCE_BH,
        VL53L5CX_REFLECTANCE_BH,
        VL53L5CX_TARGET_STATUS_BH,
        VL53L5CX_MOTION_DETECT_BH};

    /* Enable selected outputs in the 'platform.h' file */
#ifndef VL53L5CX_DISABLE_AMBIENT_PER_SPAD
    output_bh_enable[0] += (uint32_t)8;
#endif
#ifndef VL53L5CX_DISABLE_NB_SPADS_ENABLED
    output_bh_enable[0] += (uint32_t)16;
#endif
#ifndef VL53L5CX_DISABLE_NB_TARGET_DETECTED
    output_bh_enable[0] += (uint32_t)32;
#endif
#ifndef VL53L5CX_DISABLE_SIGNAL_PER_SPAD
    output_bh_enable[0] += (uint32_t)64;
#endif
#ifndef VL53L5CX_DISABLE_RANGE_SIGMA_MM
    output_bh_enable[0] += (uint32_t)128;
#endif
#ifndef VL53L5CX_DISABLE_DISTANCE_MM
    output_bh_enable[0] += (uint32_t)256;
#endif
#ifndef VL53L5CX_DISABLE_REFLECTANCE_PERCENT
    output_bh_enable[0] += (uint32_t)512;
#endif
#ifndef VL53L5CX_DISABLE_TARGET_STATUS
    output_bh_enable[0] += (uint32_t)1024;
#endif
#ifndef VL53L5CX_DISABLE_MOTION_INDICATOR
    output_bh_enable[0] += (uint32_t)2048;
#endif

    /* Update data size */
    for (i = 0; i < (uint32_t)(sizeof(output)/sizeof(uint32_t)); i++)
    {
        if ((output[i] == (uint8_t)0) 
                || ((output_bh_enable[i/(uint32_t)32]
                        &((uint32_t)1 << (i%(uint32_t)32))) == (uint32_t)0))
        {
            continue;
        }

        bh_ptr = (union Block_header *)&(output[i]);
        if (((uint8_t)bh_ptr->type >= (uint8_t)0x1) 
                && ((uint8_t)bh_ptr->type < (uint8_t)0x0d))
        {
            if ((bh_ptr->idx >= (uint16_t)0x54d0) 
                    && (bh_ptr->idx < (uint16_t)(0x54d0 + 960)))
            {
                bh_ptr->size = resolution;
            }
            else
            {
                bh_ptr->size = (uint8_t)(resolution 
                        * (uint8_t)VL53L5CX_NB_TARGET_PER_ZONE);
            }
            p_dev->data_read_size += bh_ptr->type * bh_ptr->size;
        }
        else
        {
            p_dev->data_read_size += bh_ptr->size;
        }
        p_dev->data_read_size += (uint32_t)4;
    }
    p_dev->data_read_size += (uint32_t)20;

    status |= vl53l5cx_dci_write_data(p_dev,
            (uint8_t*)&(output), VL53L5CX_DCI_OUTPUT_LIST,
            (uint16_t)sizeof(output));

    header_config[0] = p_dev->data_read_size;
    header_config[1] = i + (uint32_t)1;

    status |= vl53l5cx_dci_write_data(p_dev,
            (uint8_t*)&(header_config), VL53L5CX_DCI_OUTPUT_CONFIG,
            (uint16_t)sizeof(header_config));

    status |= vl53l5cx_dci_write_data(p_dev,
            (uint8_t*)&(output_bh_enable), VL53L5CX_DCI_OUTPUT_ENABLES,
            (uint16_t)sizeof(output_bh_enable));

    /* Start xshut bypass (interrupt mode) */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x09, 0x05);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

    /* Start ranging session */
    status |= WrMulti(&(p_dev->platform), VL53L5CX_UI_CMD_END - 
            (uint16_t)(4 - 1), (uint8_t*)cmd, sizeof(cmd));
    status |= _vl53l5cx_poll_for_answer(p_dev, 4, 1,
            VL53L5CX_UI_CMD_STATUS, 0xff, 0x03);

    return status;

} // vl53l5cx_set_power_mode

uint8_t vl53l5cx_stop_ranging(
        VL53L5CX_Configuration		*p_dev)
{
    uint8_t tmp = 0, status = VL53L5CX_STATUS_OK;
    uint16_t timeout = 0;
    uint32_t auto_stop_flag = 0;

    status |= RdMulti(&(p_dev->platform),
            0x2FFC, (uint8_t*)&auto_stop_flag, 4);
    if(auto_stop_flag != (uint32_t)0x4FF)
    {
        status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);

        /* Provoke MCU stop */
        status |= WrByte(&(p_dev->platform), 0x15, 0x16);
        status |= WrByte(&(p_dev->platform), 0x14, 0x01);

        /* Poll for G02 status 0 MCU stop */
        while(((tmp & (uint8_t)0x80) >> 7) == (uint8_t)0x00)
        {
            status |= RdByte(&(p_dev->platform), 0x6, &tmp);
            status |= WaitMs(&(p_dev->platform), 10);
            timeout++;
            /* Timeout reached after 5 seconds */
            if(timeout > (uint16_t)500)
            {
                status = VL53L5CX_STATUS_ERROR;
            }
        }
    }
    /* Undo MCU stop */
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
    status |= WrByte(&(p_dev->platform), 0x14, 0x00);
    status |= WrByte(&(p_dev->platform), 0x15, 0x00);

    /* Stop xshut bypass */
    status |= WrByte(&(p_dev->platform), 0x09, 0x04);
    status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

    return status;
} //  vl53l5cx_stop_ranging

uint8_t vl53l5cx_check_data_ready(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				*p_isReady)
{
    uint8_t status = VL53L5CX_STATUS_OK;

    status |= RdMulti(&(p_dev->platform), 0x0, p_dev->temp_buffer, 4);

    if((p_dev->temp_buffer[0] != p_dev->streamcount)
            && (p_dev->temp_buffer[0] != (uint8_t)255)
            && (p_dev->temp_buffer[1] == (uint8_t)0x5)
            && ((p_dev->temp_buffer[2] & (uint8_t)0x5) == (uint8_t)0x5)
            && ((p_dev->temp_buffer[3] & (uint8_t)0x10) ==(uint8_t)0x10)
      )
    {
        *p_isReady = (uint8_t)1;
        p_dev->streamcount = p_dev->temp_buffer[0];
    }
    else
    {
        *p_isReady = 0;
    }

    return status;

} // vl53l5cx_check_data_ready

uint8_t vl53l5cx_get_ranging_data(
        VL53L5CX_Configuration		*p_dev,
        VL53L5CX_ResultsData		*p_results)
{
    uint8_t status = VL53L5CX_STATUS_OK;
    union Block_header *bh_ptr;
    uint32_t i, j, msize;

    status |= RdMulti(&(p_dev->platform), 0x0,
            p_dev->temp_buffer, p_dev->data_read_size);

    SwapBuffer(p_dev->temp_buffer, (uint16_t)p_dev->data_read_size);


    /* Start conversion at position 16 to avoid headers */
    for (i = (uint32_t)16; i 
            < (uint32_t)p_dev->data_read_size; i+=(uint32_t)4)
    {
        bh_ptr = (union Block_header *)&(p_dev->temp_buffer[i]);
        if ((bh_ptr->type > (uint32_t)0x1) 
                && (bh_ptr->type < (uint32_t)0xd))
        {
            msize = bh_ptr->type * bh_ptr->size;
        }
        else
        {
            msize = bh_ptr->size;
        }

        switch(bh_ptr->idx){
#ifndef VL53L5CX_DISABLE_AMBIENT_PER_SPAD
            case VL53L5CX_AMBIENT_RATE_IDX:
                (void)memcpy(p_results->ambient_per_spad,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_NB_SPADS_ENABLED
            case VL53L5CX_SPAD_COUNT_IDX:
                (void)memcpy(p_results->nb_spads_enabled,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_NB_TARGET_DETECTED
            case VL53L5CX_NB_TARGET_DETECTED_IDX:
                (void)memcpy(p_results->nb_target_detected,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_SIGNAL_PER_SPAD
            case VL53L5CX_SIGNAL_RATE_IDX:
                (void)memcpy(p_results->signal_per_spad,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_RANGE_SIGMA_MM
            case VL53L5CX_RANGE_SIGMA_MM_IDX:
                (void)memcpy(p_results->range_sigma_mm,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_DISTANCE_MM
            case VL53L5CX_DISTANCE_IDX:
                (void)memcpy(p_results->distance_mm,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_REFLECTANCE_PERCENT
            case VL53L5CX_REFLECTANCE_EST_PC_IDX:
                (void)memcpy(p_results->reflectance,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_TARGET_STATUS
            case VL53L5CX_TARGET_STATUS_IDX:
                (void)memcpy(p_results->target_status,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
#ifndef VL53L5CX_DISABLE_MOTION_INDICATOR
            case VL53L5CX_MOTION_DETEC_IDX:
                (void)memcpy(&p_results->motion_indicator,
                        &(p_dev->temp_buffer[i + (uint32_t)4]), msize);
                break;
#endif
            default:
                break;
        }
        i += msize;
    }

    /* Convert data into their real format */
#ifndef VL53L5CX_DISABLE_AMBIENT_PER_SPAD
    for(i = 0; i < (uint32_t)VL53L5CX_RESOLUTION_8X8; i++)
    {
        p_results->ambient_per_spad[i] /= (uint32_t)2048;
    }
#endif

    for(i = 0; i < (uint32_t)(VL53L5CX_RESOLUTION_8X8
                *VL53L5CX_NB_TARGET_PER_ZONE); i++)
    {
#ifndef VL53L5CX_DISABLE_DISTANCE_MM
        p_results->distance_mm[i] /= 4;
        if(p_results->distance_mm[i] < 0)
        {
            p_results->distance_mm[i] = 0;
        }
#endif
#ifndef VL53L5CX_DISABLE_RANGE_SIGMA_MM
        p_results->range_sigma_mm[i] /= (uint16_t)128;
#endif
#ifndef VL53L5CX_DISABLE_SIGNAL_PER_SPAD
        p_results->signal_per_spad[i] /= (uint32_t)2048;
#endif
    }

    /* Set target status to 255 if no target is detected for this zone */
#ifndef VL53L5CX_DISABLE_NB_TARGET_DETECTED
    for(i = 0; i < (uint32_t)VL53L5CX_RESOLUTION_8X8; i++)
    {
        if(p_results->nb_target_detected[i] == (uint8_t)0){
            for(j = 0; j < (uint32_t)
                    VL53L5CX_NB_TARGET_PER_ZONE; j++)
            {
#ifndef VL53L5CX_DISABLE_TARGET_STATUS
                p_results->target_status
                    [((uint32_t)VL53L5CX_NB_TARGET_PER_ZONE
                            *(uint32_t)i) + j]=(uint8_t)255;
#endif
            }
        }
    }
#endif

#ifndef VL53L5CX_DISABLE_MOTION_INDICATOR
    for(i = 0; i < (uint32_t)32; i++)
    {
        p_results->motion_indicator.motion[i] /= (uint32_t)65535;
    }
#endif

#ifdef VL53L5CX_LTF_FILTER
    // Discard false tarets of the tail
    status |= _vl53l5cx_remove_false_target(p_dev, p_results);
#endif

    return status;

} // vl53l5cx_get_ranging_data

uint8_t vl53l5cx_get_resolution(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				*p_resolution)
{
    uint8_t status = VL53L5CX_STATUS_OK;

    status |= vl53l5cx_dci_read_data(p_dev, p_dev->temp_buffer,
            VL53L5CX_DCI_ZONE_CONFIG, 8);
    *p_resolution = p_dev->temp_buffer[0x00]*p_dev->temp_buffer[0x01];

    return status;

} // vl53l5cx_get_resolution



uint8_t vl53l5cx_set_resolution(
        VL53L5CX_Configuration 		 *p_dev,
        uint8_t				resolution)
{
    uint8_t status = VL53L5CX_STATUS_OK;

    switch(resolution){
        case VL53L5CX_RESOLUTION_4X4:
            status |= vl53l5cx_dci_read_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_DSS_CONFIG, 16);
            p_dev->temp_buffer[0x04] = 64;
            p_dev->temp_buffer[0x06] = 64;
            p_dev->temp_buffer[0x09] = 4;
            status |= vl53l5cx_dci_write_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_DSS_CONFIG, 16);

            status |= vl53l5cx_dci_read_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_ZONE_CONFIG, 8);
            p_dev->temp_buffer[0x00] = 4;
            p_dev->temp_buffer[0x01] = 4;
            p_dev->temp_buffer[0x04] = 8;
            p_dev->temp_buffer[0x05] = 8;
            status |= vl53l5cx_dci_write_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_ZONE_CONFIG, 8);
            break;

        case VL53L5CX_RESOLUTION_8X8:
            status |= vl53l5cx_dci_read_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_DSS_CONFIG, 16);
            p_dev->temp_buffer[0x04] = 16;
            p_dev->temp_buffer[0x06] = 16;
            p_dev->temp_buffer[0x09] = 1;
            status |= vl53l5cx_dci_write_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_DSS_CONFIG, 16);

            status |= vl53l5cx_dci_read_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_ZONE_CONFIG, 8);
            p_dev->temp_buffer[0x00] = 8;
            p_dev->temp_buffer[0x01] = 8;
            p_dev->temp_buffer[0x04] = 4;
            p_dev->temp_buffer[0x05] = 4;
            status |= vl53l5cx_dci_write_data(p_dev,
                    p_dev->temp_buffer,
                    VL53L5CX_DCI_ZONE_CONFIG, 8);

            break;

        default:
            status = VL53L5CX_STATUS_INVALID_PARAM;
            break;
    }

    status |= _vl53l5cx_send_offset_data(p_dev, resolution);
    status |= _vl53l5cx_send_xtalk_data(p_dev, resolution);

#ifdef VL53L5CX_LTF_FILTER
    if (status == VL53L5CX_STATUS_OK)
        p_dev->resolution = resolution;
#endif

    return status;

} // vl53l5cx_set_resolution

uint8_t vl53l5cx_get_ranging_frequency_hz(
        VL53L5CX_Configuration		*p_dev,
        uint8_t				*p_frequency_hz)
{
    uint8_t status = VL53L5CX_STATUS_OK;

    status |= vl53l5cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
            VL53L5CX_DCI_FREQ_HZ, 4);
    *p_frequency_hz = p_dev->temp_buffer[0x01];

    return status;

} // vl53l5cx_get_ranging_frequency_hz

uint8_t vl53l5cx_set_ranging_frequency_hz(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				frequency_hz)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_replace_data(p_dev, p_dev->temp_buffer,
					VL53L5CX_DCI_FREQ_HZ, 4,
					(uint8_t*)&frequency_hz, 1, 0x01);

	return status;

} // vl53l5cx_set_ranging_frequency_hz

uint8_t vl53l5cx_get_integration_time_ms(
		VL53L5CX_Configuration		*p_dev,
		uint32_t			*p_time_ms)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
			VL53L5CX_DCI_INT_TIME, 20);

	(void)memcpy(p_time_ms, &(p_dev->temp_buffer[0x0]), 4);
	*p_time_ms /= (uint32_t)1000;

	return status;

} // vl53l5cx_get_integration_time_ms

uint8_t vl53l5cx_set_integration_time_ms(
		VL53L5CX_Configuration		*p_dev,
		uint32_t			integration_time_ms)
{
	uint8_t status = VL53L5CX_STATUS_OK;
        uint32_t integration = integration_time_ms;

	/* Integration time must be between 2ms and 1000ms */
	if((integration < (uint32_t)2)
           || (integration > (uint32_t)1000))
	{
		status |= VL53L5CX_STATUS_INVALID_PARAM;
	}else
	{
		integration *= (uint32_t)1000;

		status |= vl53l5cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L5CX_DCI_INT_TIME, 20,
				(uint8_t*)&integration, 4, 0x00);
	}

	return status;

} //vl53l5cx_set_integration_time_ms

uint8_t vl53l5cx_get_sharpener_percent(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*p_sharpener_percent)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_read_data(p_dev,p_dev->temp_buffer,
			VL53L5CX_DCI_SHARPENER, 16);

	*p_sharpener_percent = (p_dev->temp_buffer[0xD]
                                *(uint8_t)100)/(uint8_t)255;

	return status;

} // vl53l5cx_get_sharpener_percent

uint8_t vl53l5cx_set_sharpener_percent(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				sharpener_percent)
{
	uint8_t status = VL53L5CX_STATUS_OK;
        uint8_t sharpener;

	if(sharpener_percent >= (uint8_t)100)
	{
		status |= VL53L5CX_STATUS_INVALID_PARAM;
	}
	else
	{
		sharpener = (sharpener_percent*(uint8_t)255)/(uint8_t)100;
		status |= vl53l5cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L5CX_DCI_SHARPENER, 16,
                                (uint8_t*)&sharpener, 1, 0xD);
	}

	return status;

} // vl53l5cx_set_sharpener_percent

uint8_t vl53l5cx_get_target_order(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*p_target_order)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
			VL53L5CX_DCI_TARGET_ORDER, 4);
	*p_target_order = (uint8_t)p_dev->temp_buffer[0x0];

	return status;

} // vl53l5cx_get_target_order

uint8_t vl53l5cx_set_target_order(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				target_order)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	if((target_order == (uint8_t)VL53L5CX_TARGET_ORDER_CLOSEST)
		|| (target_order == (uint8_t)VL53L5CX_TARGET_ORDER_STRONGEST))
	{
		status |= vl53l5cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L5CX_DCI_TARGET_ORDER, 4,
                                (uint8_t*)&target_order, 1, 0x0);
#ifdef VL53L5CX_LTF_FILTER
		if (status == VL53L5CX_STATUS_OK)
			p_dev->target_order = target_order;
#endif
	}else
	{
		status |= VL53L5CX_STATUS_INVALID_PARAM;
	}

	return status;

} // vl53l5cx_set_target_order

uint8_t vl53l5cx_get_ranging_mode(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*p_ranging_mode)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L5CX_DCI_RANGING_MODE, 8);

	if(p_dev->temp_buffer[0x01] == (uint8_t)0x1)
	{
		*p_ranging_mode = VL53L5CX_RANGING_MODE_CONTINUOUS;
	}
	else
	{
		*p_ranging_mode = VL53L5CX_RANGING_MODE_AUTONOMOUS;
	}

	return status;

} // vl53l5cx_get_ranging_mode

uint8_t vl53l5cx_set_ranging_mode(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				ranging_mode)
{
	uint8_t status = VL53L5CX_STATUS_OK;
	uint32_t single_range = 0x00;

	status |= vl53l5cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L5CX_DCI_RANGING_MODE, 8);

	switch(ranging_mode)
	{
		case VL53L5CX_RANGING_MODE_CONTINUOUS:
			p_dev->temp_buffer[0x01] = 0x1;
			p_dev->temp_buffer[0x03] = 0x3;
			single_range = 0x00;
			break;

		case VL53L5CX_RANGING_MODE_AUTONOMOUS:
			p_dev->temp_buffer[0x01] = 0x3;
			p_dev->temp_buffer[0x03] = 0x2;
			single_range = 0x01;
			break;

		default:
			status = VL53L5CX_STATUS_INVALID_PARAM;
			break;
	}

	status |= vl53l5cx_dci_write_data(p_dev, p_dev->temp_buffer,
			VL53L5CX_DCI_RANGING_MODE, (uint16_t)8);

	status |= vl53l5cx_dci_write_data(p_dev, (uint8_t*)&single_range,
			VL53L5CX_DCI_SINGLE_RANGE, 
                        (uint16_t)sizeof(single_range));

	return status;

} // vl53l5cx_set_ranging_mode

uint8_t vl53l5cx_dci_read_data(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size)
{
	int16_t i;
	uint8_t status = VL53L5CX_STATUS_OK;
        uint32_t rd_size = (uint32_t) data_size + (uint32_t)12;
	uint8_t cmd[] = {0x00, 0x00, 0x00, 0x00,
			0x00, 0x00, 0x00, 0x0f,
			0x00, 0x02, 0x00, 0x08};

	/* Check if tmp buffer is large enough */
	if((data_size + (uint16_t)12)>(uint16_t)VL53L5CX_TEMPORARY_BUFFER_SIZE)
	{
		status |= VL53L5CX_STATUS_ERROR;
	}
	else
	{
		cmd[0] = (uint8_t)(index >> 8);	
		cmd[1] = (uint8_t)(index & (uint32_t)0xff);			
		cmd[2] = (uint8_t)((data_size & (uint16_t)0xff0) >> 4);
		cmd[3] = (uint8_t)((data_size & (uint16_t)0xf) << 4);

	/* Request data reading from FW */
		status |= WrMulti(&(p_dev->platform),
			(VL53L5CX_UI_CMD_END-(uint16_t)11),cmd, sizeof(cmd));
		status |= _vl53l5cx_poll_for_answer(p_dev, 4, 1,
			VL53L5CX_UI_CMD_STATUS,
			0xff, 0x03);

	/* Read new data sent (4 bytes header + data_size + 8 bytes footer) */
		status |= RdMulti(&(p_dev->platform), VL53L5CX_UI_CMD_START,
			p_dev->temp_buffer, rd_size);
		SwapBuffer(p_dev->temp_buffer, data_size + (uint16_t)12);

	/* Copy data from FW into input structure (-4 bytes to remove header) */
		for(i = 0 ; i < (int16_t)data_size;i++){
			data[i] = p_dev->temp_buffer[i + 4];
		}
	}

	return status;

} // vl53l5cx_dci_read_data

uint8_t vl53l5cx_dci_write_data(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size)
{
	uint8_t status = VL53L5CX_STATUS_OK;
	int16_t i;

	uint8_t headers[] = {0x00, 0x00, 0x00, 0x00};
	uint8_t footer[] = {0x00, 0x00, 0x00, 0x0f, 0x05, 0x01,
			(uint8_t)((data_size + (uint16_t)8) >> 8), 
			(uint8_t)((data_size + (uint16_t)8) & (uint8_t)0xFF)};

	uint16_t address = (uint16_t)VL53L5CX_UI_CMD_END - 
		(data_size + (uint16_t)12) + (uint16_t)1;

	/* Check if cmd buffer is large enough */
	if((data_size + (uint16_t)12) 
           > (uint16_t)VL53L5CX_TEMPORARY_BUFFER_SIZE)
	{
		status |= VL53L5CX_STATUS_ERROR;
	}
	else
	{
		headers[0] = (uint8_t)(index >> 8);
		headers[1] = (uint8_t)(index & (uint32_t)0xff);
		headers[2] = (uint8_t)(((data_size & (uint16_t)0xff0) >> 4));
		headers[3] = (uint8_t)((data_size & (uint16_t)0xf) << 4);

	/* Copy data from structure to FW format (+4 bytes to add header) */
		SwapBuffer(data, data_size);
		for(i = (int16_t)data_size - (int16_t)1 ; i >= 0; i--)
		{
			p_dev->temp_buffer[i + 4] = data[i];
		}

	/* Add headers and footer */
		(void)memcpy(&p_dev->temp_buffer[0], headers, sizeof(headers));
		(void)memcpy(&p_dev->temp_buffer[data_size + (uint16_t)4],
			footer, sizeof(footer));

	/* Send data to FW */
		status |= WrMulti(&(p_dev->platform),address,
			p_dev->temp_buffer,
			(uint32_t)((uint32_t)data_size + (uint32_t)12));
		status |= _vl53l5cx_poll_for_answer(p_dev, 4, 1,
			VL53L5CX_UI_CMD_STATUS, 0xff, 0x03);

		SwapBuffer(data, data_size);
	}

	return status;

} // vl53l5cx_dci_write_data

uint8_t vl53l5cx_dci_replace_data(
		VL53L5CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size,
		uint8_t				*new_data,
		uint16_t			new_data_size,
		uint16_t			new_data_pos)
{
	uint8_t status = VL53L5CX_STATUS_OK;

	status |= vl53l5cx_dci_read_data(p_dev, data, index, data_size);
	(void)memcpy(&(data[new_data_pos]), new_data, new_data_size);
	status |= vl53l5cx_dci_write_data(p_dev, data, index, data_size);

	return status;

} // vl53l5cx_dci_replace_data
